/*
 * commands.c
 *
 * Copyright 2011 Pieter Agten
 *
 * This file is part of Yarf.
 *
 * Yarf is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Yarf is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Yarf.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * @file   commands.c
 * @author Pieter Agten (pieter.agten@gmail.com)
 * @date   21 sep 2011
 * @brief  Implementation of g-code commands.
 */

#include "commands.h"

#include "movement/planner.h"
#include "movement/block_handler.h"
#include "hardware/steppers.h"
#include "temperature/temperature.h"
#include "util/delays.h"

#include <math.h>
#include <stdbool.h>

/**
 * The lowest possible temperature that can be set for the nozzle and the print
 * bed in degrees celcius.
 */
#define MIN_TEMP_C  0.0

/**
 * The highest possible temperature that can be set for the nozzle and the
 * print bed in degrees celcius.
 */
#define MAX_TEMP_C  300.0


/**
 * Whether the received coordinates are absolute or relative to the current position
 */
static bool use_relative_coordinates;

/**
 * The feed rate of the last received movement.
 */
static speed_t last_feed_rate;

/**
 * Initializes a cmd_reponse_t structure to be an 'OK' response.
 *
 * @param r pointer to the cmd_response_t structure to initialize
 */
static void
ok(cmd_response_t *r)
{
  r->status = OK;
  r->T = NAN;
  r->B = NAN;
  r->X = NAN;
  r->Y = NAN;
  r->Z = NAN;
  r->E = NAN;
  r->info = NULL;
}

/**
 * Initializes a cmd_reponse_t structure to be an 'invalid' response.
 *
 * @param r    pointer to the cmd_response_t structure to initialize
 * @param info pointer to a string which provides extra information on the
 *             reason of the failure
 */
static void
invalid(cmd_response_t *r, char *info)
{
  r->status = INVALID;
  r->T = NAN;
  r->B = NAN;
  r->X = NAN;
  r->Y = NAN;
  r->Z = NAN;
  r->E = NAN;
  r->info = info;
}

/**
 * Initializes a cmd_reponse_t structure to be a 'temperatures' response.
 *
 * @param r pointer to the cmd_response_t structure to initialize
 * @param t temperature value of the nozzle, in degrees celcius
 * @param b temperature value of the print bed, in degrees celcius
 */
static void
temperatures(cmd_response_t *r, float t, float b)
{
  r->status = OK;
  r->T = t;
  r->B = b;
  r->X = NAN;
  r->Y = NAN;
  r->Z = NAN;
  r->E = NAN;
  r->info = NULL;
}

/**
 * Initializes a cmd_reponse_t structure to be a 'coordinates' response.
 *
 * @param r pointer to the cmd_response_t structure to initialize
 * @param x x coordinate to put in the response
 * @param y y coordinate to put in the response
 * @param z z coordinate to put in the response
 * @param e e coordinate to put in the response
 */
static void
coordinates(cmd_response_t *r, float x, float y, float z, float e)
{
  r->status = OK;
  r->T = NAN;
  r->B = NAN;
  r->X = x;
  r->Y = y;
  r->Z = z;
  r->E = e;
  r->info = NULL;
}


void
cmd_init()
{
  use_relative_coordinates = false;
  last_feed_rate = START_FEED_RATE;
}


void
cmd_controlled_move(cmd_response_t *r, float x, float y, float z, float e, float f)
{
  steps_t steps_x, steps_y, steps_z, steps_e;
  speed_t feed_rate;
  if (use_relative_coordinates) {
    steps_x = isnan(x) ? 0 : lround(x * X_STEPS_PER_MM);
    steps_y = isnan(y) ? 0 : lround(y * Y_STEPS_PER_MM);
    steps_z = isnan(z) ? 0 : lround(z * Z_STEPS_PER_MM);
    steps_e = isnan(e) ? 0 : lround(e * E_STEPS_PER_MM);
    feed_rate = isnan(f) ? 0 : lround(f);
    plan_linear_movement_rel(steps_x,steps_y,steps_z,steps_e,feed_rate);
  } else {
    steps_x = isnan(x) ? plan_get_virtual_position(X_AXIS) : lround(x * X_STEPS_PER_MM);
    steps_y = isnan(y) ? plan_get_virtual_position(Y_AXIS) : lround(y * Y_STEPS_PER_MM);
    steps_z = isnan(z) ? plan_get_virtual_position(Z_AXIS) : lround(z * Z_STEPS_PER_MM);
    steps_e = isnan(e) ? plan_get_virtual_position(E_AXIS) : lround(e * E_STEPS_PER_MM);
    feed_rate = isnan(f) ? last_feed_rate : lround(f);
    plan_linear_movement_abs(steps_x,steps_y,steps_z,steps_e,feed_rate);
  }
  last_feed_rate = feed_rate;
  block_handler_start();
  ok(r);
}


void 
cmd_set_units_inches(cmd_response_t *r)
{
  invalid(r,"inches are not supported");
}


void 
cmd_set_units_millimeters(cmd_response_t *r)
{
  // Units are always in millimeters
  ok(r);
}


void
cmd_move_to_origin(cmd_response_t *r, bool x, bool y, bool z)
{
  if (!x && !y && !z) {
    // Move all axes to origin
      plan_home_axes(true, true, true);
  } else {
    // Move only the specified axes to origin
    plan_home_axes(x, y, z);
  }
  block_handler_start();
  ok(r);
}


void
cmd_set_absolute_positioning(cmd_response_t *r)
{
  use_relative_coordinates = false;
  ok(r);
}


void
cmd_set_relative_positioning(cmd_response_t *r)
{
  use_relative_coordinates = true;
  ok(r);
}


void
cmd_set_position(cmd_response_t *r, float x, float y, float z, float e)
{
  if (use_relative_coordinates) {
    steps_t steps_x = isnan(x) ? 0 : lround(x * X_STEPS_PER_MM);
    steps_t steps_y = isnan(y) ? 0 : lround(y * Y_STEPS_PER_MM);
    steps_t steps_z = isnan(z) ? 0 : lround(z * Z_STEPS_PER_MM);
    steps_t steps_e = isnan(e) ? 0 : lround(e * E_STEPS_PER_MM);
    plan_set_virtual_position_rel(steps_x,steps_y,steps_z,steps_e);
  } else {
    steps_t steps_x = isnan(x) ? plan_get_virtual_position(X_AXIS) : lround(x * X_STEPS_PER_MM);
    steps_t steps_y = isnan(y) ? plan_get_virtual_position(Y_AXIS) : lround(y * Y_STEPS_PER_MM);
    steps_t steps_z = isnan(z) ? plan_get_virtual_position(Z_AXIS) : lround(z * Z_STEPS_PER_MM);
    steps_t steps_e = isnan(e) ? plan_get_virtual_position(E_AXIS) : lround(e * E_STEPS_PER_MM);
    plan_set_virtual_position_abs(steps_x,steps_y,steps_z,steps_e);
  }
  ok(r);
}


void
cmd_stop_idle_hold(cmd_response_t *r)
{
  steppers_disable();
  ok(r);
}

static bool
_is_valid_temperature(float c)
{
  return !isnan(c) && c >= MIN_TEMP_C && c <= MAX_TEMP_C;
}


void
cmd_set_extruder_temp_async(cmd_response_t *r, float c)
{
  if (_is_valid_temperature(c)) {
    temp_set_nozzle_target(c);
    ok(r);
  } else {
    invalid(r,"requested temperature invalid");
  }
}


void
cmd_read_nozzle_and_printbed_temp(cmd_response_t *r)
{
  return temperatures(r, temp_get_nozzle_temp(), temp_get_printbed_temp());
}


void
cmd_set_extruder_temp_sync(cmd_response_t *r, float c)
{
  cmd_set_extruder_temp_async(r, c);
  if (r->status == OK) {
    cmd_wait(r);
  }
}


void
cmd_set_extruder_pwm(cmd_response_t *r, float s)
{
  if (isnan(s)) {
    // The extruder stepper power is always controlled by the potentiometer on 
    // driver board
    ok(r);
  } else {
    invalid(r,"M113 with S parameter is not supported");
  }
}


void
cmd_get_position(cmd_response_t *r)
{
  coordinates(r,
    ((float)plan_get_virtual_position(X_AXIS)) / X_STEPS_PER_MM,
    ((float)plan_get_virtual_position(Y_AXIS)) / Y_STEPS_PER_MM,
    ((float)plan_get_virtual_position(Z_AXIS)) / Z_STEPS_PER_MM,
    ((float)plan_get_virtual_position(E_AXIS)) / E_STEPS_PER_MM
  );
}



/**
 * Waits for the nozzle temperature to have been reached.
 */
static void
nozzle_temperature_wait(void)
{
  while (! temp_nozzle_temperature_reached()) {
    printf("// T:%.1f B:%.1f\n",(double)temp_get_nozzle_temp(), (double)temp_get_printbed_temp());
    delay_ms_long(1000);
  }
}

/**
 * Waits for the print bed temperature to have been reached.
 */
static void
printbed_temperature_wait(void)
{
  while (! temp_printbed_temperature_reached()) {
    printf("// T:%.1f B:%.1f\n",(double)temp_get_nozzle_temp(), (double)temp_get_printbed_temp());
    delay_ms_long(1000);
  }
}


void
cmd_wait(cmd_response_t *r)
{
  if (! temp_nozzle_temperature_reached() ||
      ! temp_printbed_temperature_reached()) {
    while (! temp_nozzle_temperature_reached() || 
           ! temp_printbed_temperature_reached()) {
      nozzle_temperature_wait();
      printbed_temperature_wait();
      printf("// waiting 20s for temps to stabilize\n");
      delay_ms_long(20000); // TODO: measure how long the temperatures have been stabile instead of this simple delay
    }
  }
  
  ok(r);
}


void
cmd_set_printbed_temp_async(cmd_response_t *r, float c)
{
  if (_is_valid_temperature(c)) {
    temp_set_printbed_target(c);
    ok(r);
  } else {
    invalid(r,"requested temperature invalid");
  }
}


void
cmd_select_tool(cmd_response_t *r, int tool)
{
  if (tool == 0) {
    ok(r);
  } else {
    invalid(r,"only tool 0 is supported");
  }
}
